/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "waitinstructionlogic.h"
#include "GeneralDefine.h"
#include "programengine.h"
#include "messagelog.h"
#include "globalregister.h"

extern GlobalRegister *g_globalRegister;

#define D_WAIT_SLEEP_TIME 10000 //us
#define D_DEBUG_FLAG 0

WaitInstructionLogic::WaitInstructionLogic(int robotIdIn,ProgramSet* programSetIn,
                                           RobotIo* ioControllerIn,ProgramEngine* programEngineIn)
{
    programSet=programSetIn;
    robotId=robotIdIn;
    ioController=ioControllerIn;
    programEngine=programEngineIn;
}

E_PROGRAM_LOOP_RESULT WaitInstructionLogic::startWait(int programNum,waitInstruction waitInstruct)
{
    programEngine->updateProgramSetRegister();//不满足调节的话，会休眠D_WAIT_SLEEP_TIME，退出，下次继续执行本行。
    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;

    bool completeFlag = false;

    switch(waitInstruct.type)
    {
    case WAIT_GR_EQUAL_CONSTANT:
    {
        startWait_GrEqualConstant(programNum,waitInstruct);
        break;
    }
    case WAIT_DEAD:
    {
        break;
    }
    case WAIT_R:
    {
        float val;
        if(INDIRECT == waitInstruct.back_addtype)
        {
            if((D_ROBOT_REG_SIZE < waitInstruct.back_val) ||(D_ROBOT_REG_SIZE < programSet->regRList[waitInstruct.back_val - 1])
                    ||( 1 > waitInstruct.back_val)||(1 > programSet->regRList[waitInstruct.back_val - 1]))
            {
                programEngine->cmdError(programNum);
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3017, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                val = programSet->regRList[programSet->regRList[waitInstruct.back_val - 1] - 1];
            }
        }
        else
        {
            if(D_ROBOT_REG_SIZE < waitInstruct.back_val || 1 > waitInstruct.back_val)
            {
                programEngine->cmdError(programNum);
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3017, robotId );
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                val =  programSet->regRList[waitInstruct.back_val - 1];
//                qDebug() << __FUNCTION__<<"***line***"<<__LINE__<<"====waitInstruct.back_val===="
//                         <<waitInstruct.back_val<<"==val="<<val
//                        <<"==threadId===="<<QThread::currentThreadId()<<endl;
            }
        }
//        if(0 == val)
//        {

//        }
//        else
//        {
//            qDebug() << "=========waitInstruct.back_val============"<<waitInstruct.back_val<<"===val===="<<val<<"\n\n";
//            long time = (long)(val * 1000000);
//            realTimeSleep(time);
//        }
        completeFlag = programEngine->countWaitTime(val);
        if(completeFlag)
        {
            programEngine->calculateNextLineToRun(programNum);
        }

        break;
    }
    case WAIT_CONST_TIME:
    {
        float waitTime = (float)waitInstruct.waitTime;
//        qDebug() <<"***before**wait***"<<endl;
//        long time = (long)(waitTime * 1000000);
//        realTimeSleep(time);
//        qDebug() <<"**after***wait****"<<endl;

        completeFlag = programEngine->countWaitTime(waitTime);
        if(completeFlag)
        {
            programEngine->calculateNextLineToRun(programNum);
        }

        break;
    }
    case WAIT_R_R:
    {
        parseWaitROrConst(WAIT_CONDITION_R,WAIT_REG_R,programNum,waitInstruct);
        break;
    }
    case WAIT_R_CONST:
    {
        parseWaitROrConst(WAIT_CONDITION_R,WAIT_REG_CONST,programNum,waitInstruct);
        break;
    }
    case WAIT_AO_R:
    {
        parseWaitROrConst(WAIT_CONDITION_AO,WAIT_REG_R,programNum,waitInstruct);
        break;
    }
    case WAIT_AO_CONST:
    {
        parseWaitROrConst(WAIT_CONDITION_AO,WAIT_REG_CONST,programNum,waitInstruct);
        break;
    }
    case WAIT_AI_R:
    {
        parseWaitROrConst(WAIT_CONDITION_AI,WAIT_REG_R,programNum,waitInstruct);
        break;
    }
    case WAIT_AI_CONST:
    {
        parseWaitROrConst(WAIT_CONDITION_AI,WAIT_REG_CONST,programNum,waitInstruct);
        break;
    }
    case WAIT_GO_R:
    {
        parseWaitROrConst(WAIT_CONDITION_GO,WAIT_REG_R,programNum,waitInstruct);
        break;
    }
    case WAIT_GO_CONST:
    {
        parseWaitROrConst(WAIT_CONDITION_GO,WAIT_REG_CONST,programNum,waitInstruct);
        break;
    }
    case WAIT_GI_R:
    {
        parseWaitROrConst(WAIT_CONDITION_GI,WAIT_REG_R,programNum,waitInstruct);
        break;
    }
    case WAIT_GI_CONST:
    {
        parseWaitROrConst(WAIT_CONDITION_GI,WAIT_REG_CONST,programNum,waitInstruct);
        break;
    }
    case WAIT_DO_ON:
    {
        parseWaitIO(programNum,waitInstruct);
        break;
    }
    case WAIT_DO_OFF:
    {
        parseWaitIO(programNum,waitInstruct);
        break;
    }
    case WAIT_DO_DO:
    {
        break;
    }
    case WAIT_DO_DI:
    {
        break;
    }
    case WAIT_DO_RO:
    {
        break;
    }
    case WAIT_DO_RI:
    {
        break;
    }
    case WAIT_DO_SO:
    {
        break;
    }
    case WAIT_DO_SI:
    {
        break;
    }
    case WAIT_DO_UO:
    {
        break;
    }
    case WAIT_DO_UI:
    {
        break;
    }
    case WAIT_DO_R:
    {
        break;
    }
    case WAIT_DI_ON:
    {
        parseWaitIO(programNum,waitInstruct);
        break;
    }
    case WAIT_DI_OFF:
    {
        parseWaitIO(programNum,waitInstruct);
        break;
    }
    case WAIT_DI_DO:
    {
        break;
    }
    case WAIT_DI_DI:
    {
        break;
    }
    case WAIT_DI_RO:
    {
        break;
    }
    case WAIT_DI_RI:
    {
        break;
    }
    case WAIT_DI_SO:
    {
        break;
    }
    case WAIT_DI_SI:
    {
        break;
    }
    case WAIT_DI_UO:
    {
        break;
    }
    case WAIT_DI_UI:
    {
        break;
    }
    case WAIT_DI_R:
    {
        break;
    }
    case WAIT_RO_ON:
    {
        break;
    }
    case WAIT_RO_OFF:
    {
        break;
    }
    case WAIT_RO_DO:
    {
        break;
    }
    case WAIT_RO_DI:
    {
        break;
    }
    case WAIT_RO_RO:
    {
        break;
    }
    case WAIT_RO_RI:
    {
        break;
    }
    case WAIT_RO_SO:
    {
        break;
    }
    case WAIT_RO_SI:
    {
        break;
    }
    case WAIT_RO_UO:
    {
        break;
    }
    case WAIT_RO_UI:
    {
        break;
    }
    case WAIT_RO_R:
    {
        break;
    }
    case WAIT_RI_ON:
    {
        break;
    }
    case WAIT_RI_OFF:
    {
        break;
    }
    case WAIT_RI_DO:
    {
        break;
    }
    case WAIT_RI_DI:
    {
        break;
    }
    case WAIT_RI_RO:
    {
        break;
    }
    case WAIT_RI_RI:
    {
        break;
    }
    case WAIT_RI_SO:
    {
        break;
    }
    case WAIT_RI_SI:
    {
        break;
    }
    case WAIT_RI_UO:
    {
        break;
    }
    case WAIT_RI_UI:
    {
        break;
    }
    case WAIT_RI_R:
    {
        break;
    }
    case WAIT_SO_ON:
    {
        break;
    }
    case WAIT_SO_OFF:
    {
        break;
    }
    case WAIT_SO_DO:
    {
        break;
    }
    case WAIT_SO_DI:
    {
        break;
    }
    case WAIT_SO_RO:
    {
        break;
    }
    case WAIT_SO_RI:
    {
        break;
    }
    case WAIT_SO_SO:
    {
        break;
    }
    case WAIT_SO_SI:
    {
        break;
    }
    case WAIT_SO_UO:
    {
        break;
    }
    case WAIT_SO_UI:
    {
        break;
    }
    case WAIT_SO_R:
    {
        break;
    }
    case WAIT_SI_ON:
    {
        break;
    }
    case WAIT_SI_OFF:
    {
        break;
    }
    case WAIT_SI_DO:
    {
        break;
    }
    case WAIT_SI_DI:
    {
        break;
    }
    case WAIT_SI_RO:
    {
        break;
    }
    case WAIT_SI_RI:
    {
        break;
    }
    case WAIT_SI_SO:
    {
        break;
    }
    case WAIT_SI_SI:
    {
        break;
    }
    case WAIT_SI_UO:
    {
        break;
    }
    case WAIT_SI_UI:
    {
        break;
    }
    case WAIT_SI_R:
    {
        break;
    }
    case WAIT_UO_ON:
    {
        break;
    }
    case WAIT_UO_OFF:
    {
        break;
    }
    case WAIT_UO_DO:
    {
        break;
    }
    case WAIT_UO_DI:
    {
        break;
    }
    case WAIT_UO_RO:
    {
        break;
    }
    case WAIT_UO_RI:
    {
        break;
    }
    case WAIT_UO_SO:
    {
        break;
    }
    case WAIT_UO_SI:
    {
        break;
    }
    case WAIT_UO_UO:
    {
        break;
    }
    case WAIT_UO_UI:
    {
        break;
    }
    case WAIT_UO_R:
    {
        break;
    }
    case WAIT_UI_ON:
    {
        break;
    }
    case WAIT_UI_OFF:
    {
        break;
    }
    case WAIT_UI_DO:
    {
        break;
    }
    case WAIT_UI_DI:
    {
        break;
    }
    case WAIT_UI_RO:
    {
        break;
    }
    case WAIT_UI_RI:
    {
        break;
    }
    case WAIT_UI_SO:
    {
        break;
    }
    case WAIT_UI_SI:
    {
        break;
    }
    case WAIT_UI_UO:
    {
        break;
    }
    case WAIT_UI_UI:
    {
        break;
    }
    case WAIT_UI_R:
    {
        break;
    }
    default:
    {
        qDebug()<<"error, unknow wait condition";
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    }

    //条件没有达到时，会一直运行占用ｃｐｕ过高。
    usleep(D_WAIT_SLEEP_TIME);

    endFlag =programEngine->isTheLastInstruction(programNum);

    return endFlag;
}


void WaitInstructionLogic::processWaitTimeOut(int programNum, waitInstruction waitInstruct)
{
    float timeCount =  waitInstruct.waitTime;
    if(INDIRECT == waitInstruct.timeOutType)
    {
        if(TIMEOUT_TIME_R == waitInstruct.timeType)
        {
//            qDebug() << __FUNCTION__<<"====LINE=="<<__LINE__<<"***waitInstruct.timeType***"<<waitInstruct.timeType<<"===waitInstruct.waitTime===="<<waitInstruct.waitTime<<"----programSet->r[waitInstruct.waitTime - 1]--------"
//                     <<programSet->r[waitInstruct.waitTime - 1]<<"\n\n\n\n";
            if((D_ROBOT_REG_SIZE <= waitInstruct.waitTime) ||(D_ROBOT_REG_SIZE <= programSet->regRList[waitInstruct.waitTime - 1])
                    ||(1 > waitInstruct.waitTime) ||(1 > programSet->regRList[waitInstruct.waitTime - 1]))
            {
                programEngine->cmdError(programNum);
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
                return;
            }
            else
            {
                timeCount = programSet->regRList[programSet->regRList[waitInstruct.waitTime - 1] - 1];
            }
        }
        else if(TIMEOUT_TIME_AR == waitInstruct.timeType)
        {
            if((D_ROBOT_REG_SIZE <= waitInstruct.waitTime) ||(AR_SIZE <= programSet->regRList[waitInstruct.waitTime - 1])
                    ||(1 > waitInstruct.waitTime) ||(1 > programSet->regRList[waitInstruct.waitTime - 1]))
            {
                programEngine->cmdError(programNum);
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
                return;
            }
            else
            {
                timeCount = programSet->arList[programSet->regRList[waitInstruct.waitTime - 1] - 1];
            }
        }
    }
    else if(DIRECT == waitInstruct.timeOutType)
    {
//        qDebug() << __FUNCTION__<<"====LINE=="<<__LINE__<<"***waitInstruct.timeOutType***"<<waitInstruct.timeOutType<<"===waitInstruct.waitTime===="<<waitInstruct.waitTime<<"----programSet->r[waitInstruct.waitTime - 1]--------"
//                 <<programSet->r[waitInstruct.waitTime - 1]<<"\n\n\n\n";
        if(D_ROBOT_REG_SIZE < waitInstruct.waitTime || (1 > waitInstruct.waitTime))
        {
            programEngine->cmdError(programNum);
            addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
            return;
        }
        else
        {
            timeCount = programSet->regRList[waitInstruct.waitTime - 1] ;
        }
    }


//    qDebug() << __FUNCTION__<<"======"<<__LINE__<<"==WAITTIME=="<<timeCount<<"==FLAG==="<<programSet[programNum].timeOutFlag<<"\n\n\n\n";
    if(false == (*programSet)[programNum].timeOutFlag)
    {
        struct timeval tmpTime;
        gettimeofday(&tmpTime,NULL);
        (*programSet)[programNum].timeOutFlag = true;
        (*programSet)[programNum].waitTimeStart = tmpTime;
    }
    else
    {
        struct timeval tmpEndTime;
        gettimeofday(&tmpEndTime,NULL);

        float timeOutCount = 0.0;

        timeOutCount = tmpEndTime.tv_sec + tmpEndTime.tv_usec/MICROSECOND_TIME
                - (*programSet)[programNum].waitTimeStart.tv_sec - (*programSet)[programNum].waitTimeStart.tv_usec/MICROSECOND_TIME;
        if(timeOutCount >= timeCount)
        {
            (*programSet)[programNum].timeOutFlag = false;
//            qDebug() << __FUNCTION__<<"======"<<__LINE__<<"==WAITTIME=="<<timeCount<<"==timeoutFlag==="<<programSet[programNum].timeOutFlag<<"\n\n\n\n";
            QString tmpjumpLabel=waitInstruct.jumpProcess.jumpLabelStr;
            programEngine->startJump(programNum,tmpjumpLabel);
        }
        else
        {
            //else do nothing
        }
    }
}

void WaitInstructionLogic::parseWaitIO(int programNum, waitInstruction waitInstruct)
{
    int tmpVar;

    int ioVar,ioValue;

    bool flag;

    switch(waitInstruct.type)
    {
        case WAIT_DO_ON:
        {
            switch(waitInstruct.front_addtype)
            {
                case INDIRECT:
                {

                }
                break;
                case DIRECT:
                {
                    tmpVar = waitInstruct.front_val - 1;
                }
                break;

            }
            if( ioController->doCount > tmpVar )
            {
//                if(ioController->lock())
//                {
                    ioVar = ioController->ioDo[tmpVar];
//                    ioController->unlock();
//                }
//                else
//                {
//                    qDebug()<<"io 映射錯誤 ！";
//                }
            }
            else
            {
                //                QString msg=QObject::tr( "IO毫超出已有逻辑ＩＯ范围，请检查指令或ＩＯ配置，: %1   ").arg( ioController->doCount );
        //                MessageLog::getInstance()->addMessage( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3005,  robotId ,  0, 0, 0  );

                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3005, robotId ,tmpVar, ioController->doCount,(*programSet)[programNum].runningLineInt,0,(*programSet)[programNum].thisProgramName);
                programEngine->cmdError(programNum);
                return;
            }
            ioValue = true;
        }
        break;
        case WAIT_DO_OFF:
        {
            switch(waitInstruct.front_addtype)
            {
                case INDIRECT:
                {

                }
                break;
                case DIRECT:
                {
                    tmpVar = waitInstruct.front_val - 1;
//                    qDebug() << "===tmpVar ==**waitInstruct**=="<<tmpVar<<"\n\n";
                }
                break;

            }
            if( ioController->doCount > tmpVar )
            {
//                if(ioController->lock())
//                {
                    ioVar = ioController->ioDo[tmpVar];
//                    ioController->unlock();
//                }
//                else
//                {
//                    qDebug()<<"io 映射錯誤 ！";
//                }
            }
            else
            {
                //                QString msg=QObject::tr( "IO毫超出已有逻辑ＩＯ范围，请检查指令或ＩＯ配置，: %1   ").arg( ioController->doCount );
        //                MessageLog::getInstance()->addMessage( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3005,  robotId ,  0, 0, 0  );

                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3005, robotId ,tmpVar, ioController->doCount,(*programSet)[programNum].runningLineInt,0,(*programSet)[programNum].thisProgramName);
                programEngine->cmdError(programNum);
                return;
            }
            ioValue = false;
        }
        break;
        case WAIT_DI_ON:
        {
            switch(waitInstruct.front_addtype)
            {
                case INDIRECT:
                {

                }
                break;
                case DIRECT:
                {
                    tmpVar = waitInstruct.front_val - 1;
                }
                break;
            }
            ioVar = ioController->getDi(tmpVar);

            ioValue = true;
        }
        break;
        case WAIT_DI_OFF:
        {
            switch(waitInstruct.front_addtype)
            {
                case INDIRECT:
                {

                }
                break;
                case DIRECT:
                {
                    tmpVar = waitInstruct.front_val - 1;
                }
                break;
            }

            ioVar = ioController->getDi(tmpVar);
//            if( ioController->diCount > tmpVar )
//            {
//                    ioVar = ioController->ioDi[tmpVar];

//            }
//            else
//            {
//                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3005, robotId ,tmpVar,
//                        ioController->diCount,(*programSet)[programNum].runningLineInt,0,(*programSet)[programNum].thisProgramName);
//                programEngine->cmdError(programNum);
//                return;
//            }
            ioValue = false;
        }
        break;

    }



    switch(waitInstruct.operatorType)
    {
        case EQU:
        case NOT_EQU:
        {
            flag = operatorCompute(ioVar,ioValue,waitInstruct.operatorType);
        }
        break;
        default:
        {
            programEngine->cmdError(programNum);
            addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3019,robotId);
            return;
        }
    }

//    qDebug()<<__FUNCTION__<<"======="<<__LINE__<<"==tmpVar==="<<tmpVar<<"===ioVar==="<<ioVar<<"===ioValue=="
//           <<ioValue<<"===flag=="<<flag<<"\n\n\n\n";
    switch(waitInstruct.timeOutFlag)
    {
        case TIMEOUT_EXIST:
        {
            if(flag)
            {
                (*programSet)[programNum].timeOutFlag = false;
                programEngine->calculateNextLineToRun(programNum);

            }
            else
            {
                //if timeout
                processWaitTimeOut(programNum,waitInstruct);
            }
            break;
        }
        case TIMEOUT_NOT_EXIST:
        {
            if(flag)
            {
                programEngine->calculateNextLineToRun(programNum);
            }
            else
            {
                //do noting
            }
            break;
        }
    }

}

bool WaitInstructionLogic::operatorCompute(float frontVal, float backVal,IF_OPERATOR_TYPE operaType)
{
//    qDebug() << "IF_OPERATOR_TYPE="<<operaType;
    bool flag = false;
    switch (operaType)
    {
    case GREATER:
    {
        if(frontVal > backVal)
        {
            flag = true;
        }
        break;
    }
    case GREATER_EQU:
    {
        if(frontVal >= backVal)
        {
            flag = true;
        }
        if(D_POSITIVE_ZERO>frontVal && D_NEGATIVE_ZERO<frontVal &&
                D_POSITIVE_ZERO>backVal && D_NEGATIVE_ZERO<backVal)// in case of 0.00000000001
        {
            flag = true;
        }
        break;
    }
    case EQU:
    {
        if(frontVal == backVal )
        {
            flag = true;
        }
        if(D_POSITIVE_ZERO>frontVal && D_NEGATIVE_ZERO<frontVal &&
                D_POSITIVE_ZERO>backVal && D_NEGATIVE_ZERO<backVal)// in case of 0.00000000001
        {
            flag = true;
        }
        break;
    }
    case SMALLER_EQU:
    {
        if(frontVal <= backVal)
        {
            flag = true;
        }
        if(D_POSITIVE_ZERO>frontVal && D_NEGATIVE_ZERO<frontVal &&
                D_POSITIVE_ZERO>backVal && D_NEGATIVE_ZERO<backVal)// in case of 0.00000000001
        {
            flag = true;
        }
        break;
    }
    case SMALLER:
    {
        if(frontVal < backVal)
        {
            flag = true;
        }
        break;
    }
    case NOT_EQU:
    {
        if(frontVal != backVal)
        {
            flag = true;
        }
        break;
    }
    }

    return flag;
}

int WaitInstructionLogic::reset()
{

}

E_PROGRAM_LOOP_RESULT WaitInstructionLogic::startWait_GrEqualConstant(int programNum, waitInstruction waitInstruct)
{
    double tmpCurrentRegValue;
    g_globalRegister->getRegValue(waitInstruct.front_val-1,tmpCurrentRegValue);
    if(tmpCurrentRegValue==waitInstruct.back_val)
    {
        programEngine->calculateNextLineToRun(programNum);
        return E_PROGRAM_LOOP_RESULT_NORMAL;
    }

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

void WaitInstructionLogic::parseWaitROrConst(WAIT_CONDITION_TYPE rVar,WAIT_R_OR_CONST_TYPE rValue
                                             ,int programNum,waitInstruction waitInstruct)
{
//    int frontVal,backVal;//华磊 hualei modify
    float frontVal,backVal;//华磊 hualei modify
    bool flag;
    if(D_DEBUG_FLAG)
    {
        qDebug() << "WAIT_CONDITION_TYPE="<<rVar;
        qDebug() << "WAIT_R_OR_CONST_TYPE="<<rValue;
        qDebug() << "waitInstruct.front_addtype"<<waitInstruct.front_addtype;
    }

    switch(waitInstruct.front_addtype)
    {
    case INDIRECT:
    {
        if(D_DEBUG_FLAG)
        {
            qDebug() << __FUNCTION__<<"====LINE=="<<__LINE__<<"***rVar***"
                     <<rVar<<"===waitInstruct.front_val===="<<waitInstruct.front_val
                    <<"----programSet->r[waitInstruct.front_val -1]--------"
                     <<programSet->regRList[waitInstruct.front_val -1]<<"\n\n\n\n";
        }

        if(WAIT_CONDITION_R == rVar)
        {
            if((D_ROBOT_REG_SIZE < waitInstruct.front_val) ||(D_ROBOT_REG_SIZE < programSet->regRList[waitInstruct.front_val -1])
                    ||(1 > waitInstruct.front_val) ||(1 > programSet->regRList[waitInstruct.front_val -1]))
            {
                programEngine->cmdError(programNum);
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
                return;
            }
            else
            {
                frontVal = programSet->regRList[programSet->regRList[waitInstruct.front_val -1] -1];
                if(D_DEBUG_FLAG)
                {
                    qDebug() << "frontVal="<<frontVal;
                }
            }
        }
        else if(WAIT_CONDITION_AO == rVar)
        {

        }
        else if(WAIT_CONDITION_AI == rVar)
        {

        }
        else if(WAIT_CONDITION_GO == rVar)
        {

        }
        else if(WAIT_CONDITION_GI == rVar)
        {

        }
        break;
    }
    case DIRECT:
    {
        if(WAIT_CONDITION_R == rVar)
        {
            if(D_DEBUG_FLAG)
            {
                qDebug() << __FUNCTION__<<"====LINE=="<<__LINE__<<"***rVar***"<<rVar<<"===waitInstruct.front_val===="<<waitInstruct.front_val<<"----programSet->r[waitInstruct.front_val -1]--------"
                         <<programSet->regRList[waitInstruct.front_val -1]<<"\n\n\n\n";
            }

            if(D_ROBOT_REG_SIZE < waitInstruct.front_val || 1 > waitInstruct.front_val)
            {
                programEngine->cmdError(programNum);
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
                return;
            }
            else
            {
                frontVal = programSet->regRList[waitInstruct.front_val - 1];
                if(D_DEBUG_FLAG)
                {
                    qDebug() << "frontVal="<<frontVal;
                }

            }
        }
        else if(WAIT_CONDITION_AO == rVar)
        {

        }
        else if(WAIT_CONDITION_AI == rVar)
        {

        }
        else if(WAIT_CONDITION_GO == rVar)
        {

        }
        else if(WAIT_CONDITION_GI == rVar)
        {

        }
        break;
    }
    }

    if(WAIT_REG_CONST == rValue)
    {
        backVal = waitInstruct.back_val;
        if(D_DEBUG_FLAG)
        {
            qDebug() << "backVal="<<backVal;
        }

    }
    else if(WAIT_REG_R == rValue)
    {
        switch(waitInstruct.back_addtype)
        {
        case INDIRECT:
        {
            if(D_DEBUG_FLAG)
            {
                qDebug() << __FUNCTION__<<"====LINE=="<<__LINE__<<"***rValue***"<<rValue<<"===waitInstruct.back_val===="<<waitInstruct.back_val<<"----programSet->r[waitInstruct.back_val -1]--------"
                         <<programSet->regRList[waitInstruct.back_val -1]<<"\n\n\n\n";
            }

            if((D_ROBOT_REG_SIZE < waitInstruct.back_val) ||(D_ROBOT_REG_SIZE < programSet->regRList[waitInstruct.back_val -1])
                    ||(1 > waitInstruct.back_val ) || (1 > programSet->regRList[waitInstruct.back_val - 1]))
            {
                programEngine->cmdError(programNum);
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
                return;
            }
            else
            {
                backVal = programSet->regRList[programSet->regRList[waitInstruct.back_val - 1] -1];
            }
            break;
        }
        case DIRECT:
        {
            if(D_DEBUG_FLAG)
            {
                qDebug() << __FUNCTION__<<"====LINE=="<<__LINE__<<"***rValue***"<<rValue<<"===waitInstruct.back_val===="<<waitInstruct.back_val<<"----programSet->r[waitInstruct.back_val -1]--------"
                         <<programSet->regRList[waitInstruct.back_val -1]<<"\n\n\n\n";
            }

            if(D_ROBOT_REG_SIZE < waitInstruct.back_val || 1 > waitInstruct.back_val)
            {
                programEngine->cmdError(programNum);
                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3017,robotId);
                return;
            }
            else
            {
                 backVal = programSet->regRList[waitInstruct.back_val - 1];
            }
            break;
        }
        }
    }

    flag = operatorCompute(frontVal,backVal,waitInstruct.operatorType);

    if(D_DEBUG_FLAG)
    {
        qDebug() << "flag result="<<flag;


        qDebug() <<__FUNCTION__<<"====threadId===="<<QThread::currentThreadId()<<"===timeOutFlag==="
                <<waitInstruct.timeOutFlag<<"===flag=="<<flag<<"==programNum=="<<programNum<<endl;
    }


    switch(waitInstruct.timeOutFlag)
    {
        case TIMEOUT_EXIST:
        {
            if(flag)
            {
                (*programSet)[programNum].timeOutFlag = false;
                programEngine->calculateNextLineToRun(programNum);
            }
            else
            {
                processWaitTimeOut(programNum,waitInstruct);
            }
            break;
        }
        case TIMEOUT_NOT_EXIST:
        {
            if(flag)
            {
                programEngine->calculateNextLineToRun(programNum);
            }
            else
            {
                //do noting
            }
            break;
        }
    }
}



void WaitInstructionLogic::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {

    case 3005:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr("; %1,第%2行， IO号超出已有逻辑ＩＯ范围，请检查指令或ＩＯ配置，").arg(message).arg(parameter3)
                +QString::number(parameter1) + ">="
        +QString::number(parameter2);
        break;


    }
    case 3017:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",寄存器指令错误，寄存器的范围超过限定的最大范围，请修改")+message;
        break;
    }


    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}
